#include <Wire.h>

// Encoder PINs
const int encoder_motor_left = 2;
const int encoder_motor_right = 3;

// motors PINs
const int E1 = 5; //M1 Speed Control
const int E2 = 6; //M2 Speed Control
const int M1 = 4; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

// motor parameters
boolean motor_running=false, motor_step_counting=false;

// 6.5 is diameter of wheel
// 20 is encoder step
const float _mywheel_step = (PI*6.5f)/20.0f;

// stock of step :)
long _encoder_step_left = 0, _encoder_step_right = 0;

// distance paramaters
float body_lastmove_distance_left=0.0f;
float body_lastmove_distance_right=0.0f;

//////////////
// encoders
void reinit_encoders()
{
  _encoder_step_left = _encoder_step_right = 0;
}
void setup_encoders(void)
{
  pinMode(encoder_motor_left, INPUT);
  pinMode(encoder_motor_right, INPUT);
  reinit_encoders();
  attachInterrupt(0, read_encoder_left, CHANGE);  
  attachInterrupt(1, read_encoder_right, CHANGE);
}
void read_encoder_right(void)
{
  _encoder_step_right++;
}
void read_encoder_left(void)
{
  _encoder_step_left++;
}

///////////////
// motor
void motor_stop(void)
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
  motor_running = false;

  if(motor_step_counting)
  {
    motor_step_counting = false;
    body_lastmove_distance_left = _mywheel_step*(float)_encoder_step_left;
    body_lastmove_distance_right = _mywheel_step*(float)_encoder_step_right;
    send_body_moving_distance();
    reinit_encoders();
  }
}
void motor_action(const char a, const char b, const boolean a2, const boolean b2)
{
  motor_running = true;
  if(a2==b2)
    motor_step_counting = true;
  analogWrite (E1,a);
  digitalWrite(M1,a2?HIGH:LOW);
  analogWrite (E2,b);
  digitalWrite(M2,b2?HIGH:LOW);
}
void setup_motor(void)
{
  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
}
int map_power_motor(int iPercent)
{
  //return map(iPercent, 0, 100, 75, 250); // 250 is too fast encoders :(
  return map(iPercent, 0, 100, 50, 150);
}

void move_front_at_speed(const int spd)
{
  motor_action(true, true, map_power_motor(spd), map_power_motor(spd));
}

////////////////
// infos

void print_encoders(void)
{
  Serial.print("Encoder Left=");
  Serial.print(_encoder_step_left, DEC);  
  Serial.print(" Right=");
  Serial.print(_encoder_step_right, DEC);    
  Serial.println();
}

void send_body_moving_distance(void)
{
  print_encoders();
  Serial.print("Runned Distance Left=");
  Serial.print(body_lastmove_distance_left);  
  Serial.print(" Right=");
  Serial.print(body_lastmove_distance_right);
  Serial.println();
  Serial.print("Distance Average=");
  Serial.print((body_lastmove_distance_left+body_lastmove_distance_right)/2);
  Serial.println();
}

///////////////////////
// setup
void setup(void)
{
  Serial.begin(9600);
  setup_encoders();
  setup_motor();
}

boolean _encoder_intermediary_step_ = false;
unsigned long _encoders_timing_ = 0;
int _motor_power_ = 50;

///////////////////////
// Loop
void loop(void)
{
  unsigned long _time_ = millis();
  if(_encoder_intermediary_step_ && (_time_-_encoders_timing_)>500)
  {
    _encoders_timing_ = _time_;
    print_encoders();
  }

  if(Serial.available()>0)
  {
    int cmd = Serial.read();
    if(cmd == 'r' || cmd == 'R')
    {
      // reinit encoders
      reinit_encoders();
    }
    else if(cmd == 'i' || cmd == 'I')
    {
      // print encoders info
      _encoder_intermediary_step_ = !_encoder_intermediary_step_;
    }
    else if(cmd == '1')
    {
      // speed at 10%
      _motor_power_ = 10;
      Serial.println("Set speed at 10%");
    }
    else if(cmd == '2')
    {
      // speed at 20%
      _motor_power_ = 20;
      Serial.println("Set speed at 20%");
    }
    else if(cmd == '3')
    {
      // speed at 30%
      _motor_power_ = 30;
      Serial.println("Set speed at 30%");
    }
    else if(cmd == '5')
    {
      // speed at 50%
      _motor_power_ = 50;
      Serial.println("Set speed at 50%");
    }
    else if(cmd == 'z' || cmd == 'Z')
    {
      Serial.print("Run front at ");
      Serial.print(_motor_power_, DEC);
      Serial.println("%");      
      move_front_at_speed(_motor_power_);
    }
    else if(cmd == ' ')
    {
      Serial.println("Stop"); 
      motor_stop();
    }
  }
}


